plot(pose(1,4),pose(2,4),'k*')
pose = [A B C D];
plot(pose(1,4),pose(2,4),'k*')
figure('name','testplot');
h = figure('name','testplot');
plot(pose(1,4),pose(2,4),'k*')
h1 = figure('name','testplot_2nd');
plot(pose(1,4),pose(2,4),'k*')
figure(h1)
plot(pose(1,4),pose(2,4),'k*')
hold on
plot(pose(1,3),pose(2,3),'k*')